#include "Hexapod.h"
#include "HexapodWaveGenerator.h"
#include <math.h>

void waveGeneratorInit(WaveGenerator* wave, float period, float stancePhaseRatio, float *bias) {
	wave->startT = getsystimestamp(); // TODO
	wave->period = period;
	wave->stRatio = stancePhaseRatio;
	for(uint8_t i = 0; i < LEG_CNT; i++) {
		wave->bias[i] = bias[i];
		wave->phasePast[i] = 0.5;
		wave->contactPast[i] = 0;
	}
	wave->statusPast = SWING_ALL;
	wave->calcContactPhase = waveGeneratorCalcContactPhase;
	wave->getTstance = waveGeneratorGetTstance;
	wave->getTswing = waveGeneratorGetTswing;
}

// 相位 phase [0, 1)
// normalT [0, 1)
void waveGeneratorCalcWave(WaveGenerator* wave, float *phase, uint8_t *contact, enum WaveStatus status) {
	float period = wave->period;
	float stRatio = wave->stRatio;
	if(status == WAVE_ALL) {
		float normalT[LEG_CNT] = {0};
		float passT = getsystimestamp() - wave->startT;
		for(uint8_t i = 0; i < LEG_CNT; i++){
				normalT[i] = fmod(passT + period - period * wave->bias[i], period) / period;
				if(normalT[i] < stRatio){
						contact[i] = 1;
						phase[i] = normalT[i] / stRatio;
				}else{
						contact[i] = 0;
						phase[i] = (normalT[i] - stRatio) / (1 - stRatio);  
				}
		}
	} else if(status == SWING_ALL) {
		// 腾空
		for (uint8_t i = 0; i < LEG_CNT; i++) {
			contact[i] = 0;
			phase[i] = 0.5;
		}
  } else if(status == STANCE_ALL) {
		// 触地
		for (uint8_t i = 0; i < LEG_CNT; i++) {
			contact[i] = 1;
			phase[i] = 0.5;
		}
  }
}

void waveGeneratorCalcContactPhase(WaveGenerator* wave, float *phaseResult, uint8_t *contactResult, enum WaveStatus status) {
	waveGeneratorCalcWave(wave, wave->phase, wave->contact, status);

	if(status != wave->statusPast) {
		uint8_t sum = calSwitchStatusSum(wave);
		if(sum == 0) {
			for (uint8_t i = 0; i < LEG_CNT; i++) {
				wave->switchStatus[i] = 1;
			}
			waveGeneratorCalcWave(wave, wave->phasePast, wave->contactPast,  wave->statusPast);
			// two special case
			if( (status == STANCE_ALL) && (wave->statusPast == SWING_ALL) ){
				for (uint8_t i = 0; i < LEG_CNT; i++) {
					wave->contactPast[i] = 1;
				}
			}
			else if( (status == SWING_ALL) && (wave->statusPast == STANCE_ALL) ){
				for (uint8_t i = 0; i < LEG_CNT; i++) {
					wave->contactPast[i] = 0;
				}
			}
		}
	}
	
	if(calSwitchStatusSum(wave) != 0) {
		for(uint8_t i = 0; i < LEG_CNT; i++) {
				if(wave->contact[i] == wave->contactPast[i]) {
						wave->switchStatus[i] = 0;
				}else {
						wave->contact[i]  = wave->contactPast[i];
						wave->phase[i] = wave->phasePast[i] ;
				}
		}
		if(calSwitchStatusSum(wave) == 0) {
				wave->statusPast = status;
		}
	}

	phaseResult = wave->phase;
	contactResult = wave->contact;
		
}

float waveGeneratorGetTstance(WaveGenerator* wave) {
	return wave->period * wave->stRatio;

}

float waveGeneratorGetTswing(WaveGenerator* wave) {
	return wave->period * (1 - wave->stRatio);
}

uint8_t calSwitchStatusSum(WaveGenerator* wave) {
	uint8_t sum = 0;
	for (uint8_t i = 0; i < LEG_CNT; i++) {
		sum += wave->switchStatus[i];
	}
	return sum;
}

